clear;
exec('matrix/circshift.sci');
num_bw = 8;
num_na = 4;
bp = 0:0.5/num_bw:0.5;
order = 2*num_bw-1;
wft = zeros(1,order);
wfm = zeros(1,256);
fr = zeros(1,256);
for i=1:num_bw
    [wft_t, wfm_t, fr_t]=wfir('bp',order,[bp(i) bp(i+1)], 're',[3 1.8]);
    wft(i,:) = wft_t;
    wfm(i,:) = wfm_t;
    fr(i,:) = fr_t;
end
//plot2d('nl',fr(1,:),wfm(1,:));
//pause
//WFT = fft([wft(1,:) zeros(1,119)]);
//phase = atan(imag(WFT)./real(WFT));
//mag = abs(WFT);
//figure(); plot(fr(1,:),phase);
//figure(); plot(fr(1,:),mag);


Xadc = csvRead('Xadc20.csv');
//plot2d('gnn',fr,wfm);
//pause

x = Xadc;
[num, len] = size(x);
y = zeros(num_bw*num, len);

for i=1:num_bw
    for j=0:num-1
        y(j*num_bw+i,:) = filter(wft(i,:), 1,x(j+1,:));
    end
end
main = [1 2];
E = x(main,:);
// Надо подумать
//Z = [x ; circshift(x',1)'; circshift(x',2)'; circshift(x',3)'; circshift(x',4)'; circshift(x',5)'];
//Z(main,:) = [];
Z_ = x;
Z_(main,:) = [];
n_delay = 15;
Z = Z_;
for i = 1:n_delay
    Z = [Z ; circshift(Z_',i)'];
end

//Z = Zadc;// [Zadc; circshift(Zadc',1)'; circshift(Zadc',2)'; circshift(Zadc',3)'; circshift(Zadc',4)']
Rxx = Z*Z';
invRxx = inv(Rxx);
Rxd = Z*E';
wm = invRxx * Rxd;
out = E - wm'*Z;
db = 20*log10(stdev(E(1,:))/stdev(out(1,:)));
disp(db);

mainw = [(main(1)-1)*num_bw+1:main(2)*num_bw,];
Ew = y(mainw,:);
n_delayw = 2;
Zw = y;
Zw(mainw,:) = [];
//for i=1:n_delayw
// Zw = [Zw; circshift(Zw',i)'];
//end
outw = zeros(2*num_bw,len);
for i=1:num_bw
    Zw__ = Zw(i:num_bw:$,:);
    Zw_ = Zw__;
    Ew_ = Ew(i:num_bw:$,:);
    for j = 1:n_delayw
        Zw_ = [Zw_; circshift(Zw__',j)'];
    end
    Rxxw = Zw_*Zw_';
    invRxxw = inv(Rxxw);
    Rxdw = Zw_*Ew_';
    ww = invRxxw*Rxdw;
    outw([(2*i - 1),2*i],:) = Ew_ - ww'*Zw_;
end

//Rxxw = Zw*Zw';
//invRxxw = inv(Rxxw);
//Rxdw = Zw*Ew';
//ww = invRxxw*Rxdw;
//outw = Ew - ww'*Zw;
dbw = 20*log10(stdev(sum(Ew(1:$/2,:),'r'))/stdev(sum(outw(1:2:$,:),'r')));
disp(dbw);
dbw_ = 20*log10(stdev(Ew(1:$/2,:),'c')./stdev(outw(1:2:$,:),'c'));
//disp(dbw_);

df = 1/(4*num_bw);
f = df:2*df:0.5-df;
bar(f,dbw_,1);

//H=fft(sum(wft,'r'));
//Hphi = atan(imag(H)./real(H));
//figure(); plot(Hphi);
//
